#include "pid.h"

PID_typedef pid1;

void pid_Iint(PID_typedef *pidx,PID_typedef* pid)
{
	pidx->set =pid->set;
	pidx->real =pid->real;
	pidx->out =pid->out;
	pidx->e0 =pid->e0;
	pidx->e1 =pid->e1;
	pidx->e2 =pid->e2;
	pidx->kp =pid->kp;
	pidx->ki =pid->ki;
	pidx->kd =pid->kd;
}
void pid_Denit(void)
{
	PID_typedef pid;
	pid.set =65;
	pid.real =0;
	pid.out =0;
	pid.e0 =0;
	pid.e1 =0;
	pid.e2 =0;
	pid.kp =1.1;//up
	pid.ki =0.1;//down
	pid.kd =1;//up
	pid_Iint(&pid1,&pid);
	
}

float pid_Run(float tp,PID_typedef *pidx)
{
	pidx->e0= pidx->set-tp;
	//pidx->out += pidx->kp *(pidx->e0) - pidx->ki *pidx->index*pidx->e1 + pidx->kd *pidx->e2;
	pidx->out = pidx->kp *(pidx->e0-pidx->e1) + pidx->ki *pidx->e0+ pidx->kd *(pidx->e0+pidx->e2-2*pidx->e1);
	pidx->e2=pidx->e1;
	pidx->e1=pidx->e0;
	return pidx->out;
}

/*******************************************************************************
* Function Name  : None
* Description    : None 
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/

void pidRun(void)
{
	double aa ;
	float mm;
	static int tg=400; 
	aa=absf(Filter(read()));
	printf("aa:%.1f\n",aa);
	aa=pid_Run(aa,&pid1);
	printf("Rt:%f\n",aa);
	mm=aa*100/9;
	printf("Tg:%f\n",mm);
	tg+=(int)mm;
	if(tg>1000) tg=1000;
	if(tg<0) tg=0;
	TIM4->CCR4=tg;
}
